#ifndef __EM_PLANNER_REFERENCELINE_PROVIDER_HPP__
#define __EM_PLANNER_REFERENCELINE_PROVIDER_HPP__

#include <cmath>
#include <iostream>
#include <vector>
#include <unistd.h>
#include <Eigen/Dense>

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>

#include <boost/bind.hpp>
#include <boost/thread.hpp>

#include "./em_utils.hpp"
#include "./qp_solver.hpp"

using namespace std;
using namespace Eigen;
using namespace boost;


/**
 * 参考线生成类
 * 1. 接收全局路径点信息，并计算 heading 和 kappa
 * 2. 寻找当前小车的投影点
 * 3. 根据投影点对 global_path_vec 进行 refline 提取
 * 4. 对提取的 refline 进行平滑处理
 * 5. 发布 refline 话题消息
*/
class ReferenceLineProvider {
private:
    double car_x =0, car_y = 0;          // 小车位置
    vector<ReferencePoint> global_path_vec;
    boost::mutex global_path_mutex_;
    vector<ReferencePoint> reference_line_smooth;
    boost::mutex reference_line_mutex_;
    

    int global_path_length_ = 0;        // 全局路径长度
    int m_index = 0;                    // 投影点在 global_path 中的下标
    std::string m_index_param_name;
    int rear_point_num;                 // 后半部分 reference_line 截取点个数
    int front_point_num;
    bool need_smooth;                   // 是否需要平滑参考线

    /* QP Solver  */
    QPSolver *qp_solver;
    double w_smooth;            // 二次规划平滑权重权重系数
    double w_length;            // 二次规划紧凑代价权重系数
    double w_ref;               // 二次规划相似权重系数
    double lb, ub;              // 二次规划上下界

    /* ros var */
    ros::NodeHandle n;
    ros::Publisher reference_line_pub_;
    ros::Subscriber global_path_sub_;
    ros::Subscriber rear_pose_sub_;
    
private:
    /**
     * 参考线截取、处理、发布流程函数
     * 1. 获取投影点
     * 2. 根据匹配点index前后截取一段距离作为参考线
     * 3. 将投影点作为参考线坐标原点
     * 4. 对参考线进行平滑处理
     * 5. 发布参考线（主要用于rviz展示）
    */
    void pubReferenceLine();

    /**
     * 将参投影点设置为参考线原点
     * 主要过程：所有点的弧长减去投影点的弧长
     * @param match_index: 投影点在参考线中的下标
     * @param proj_point: 投影点信息
     * @param cur_ref_line: 参考线数据
    */
    bool setProjPointAsBegin(const int match_index, const ReferencePoint &proj_point, 
                             vector<ReferencePoint> &cur_ref_line);
    /**
     * 接收全局路径坐标点信息，可以理解为 Routing 模块
    */
    void global_path_callback(const nav_msgs::Path &msg);
    
    void rear_pose_callback(const geometry_msgs::PoseStamped &msg);

    void calculateHeadingAndS(vector<ReferencePoint> &path);
    void calculateHeading(double x1, double y1, double x2, double y2, double &theta);
    /**
     * 计算曲率和曲率对弧长的一阶导数（近似）
     * 方法：kappa = d_heading / d_s
     * 对于 A B C 三个点，要求B点的曲率，d_heading = C_heading - A_heading
     *                               d_s = dis(A, C)
     * 同理，dkappa = d(kappa) / d_s
     * 当前的global_path是首尾相连，所以第一个点和最后一个点单独处理即可
     * 此方法只是近似求解，要求参考线的点要足够密集
    */
    void calculateKappaAndDKappa(vector<ReferencePoint> &path);

public:

    /**
     * 计算点在轨迹上的投影点
     * @param x: 横坐标
     * @param y: 纵坐标
     * @param path: 要投影的轨迹
     * @param m_index: 匹配点下标（返回值）
     * @param proj_point: 投影点信息（返回值）
     * @param pre_match_index: 上次投影的匹配点（可选，提高算法速度）
    */
    bool calculateProjection(const double x, const double y, const vector<ReferencePoint> &path, 
                             int &match_index, ReferencePoint &proj_point, int pre_match_index = 0);
    
    /**
     * 全局坐标转自然坐标（需要投影点）
     * @param proj_point: 投影点信息
     * @param host: 笛卡尔坐标下的小车坐标
     * @param sl_point: 要求的 SL 坐标信息
    */
    void cartesian2Frenet(const ReferencePoint &proj_point, const Location &host, SLPoint &sl_point);

    /**
     * 自然坐标转笛卡尔坐标（需要投影点）
     * @param proj_point: 投影点信息
     * @param sl_point: SL坐标点
     * @param host: 要求的笛卡尔坐标
    */
    void frenet2Cartesian(const ReferencePoint &proj_point, const SLPoint &sl_point, Location &host);
    double NormalizeAngle(const double angle);

    ReferenceLineProvider();
    ~ReferenceLineProvider();

    void run();

    vector<ReferencePoint> getReferenceLine();

    /**
     * 笛卡尔坐标转Frenet坐标
     * @param xy_point: 需要转换的坐标信息
     * @param path: 参考线轨迹
     * @param sl_point: sl坐标信息
    */
    bool XY2SL(const Location &xy_point, const vector<ReferencePoint> &path, SLPoint &sl_point);

};

#endif